﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using RosSharp;
using System.Threading;
using System.Net;
using System.Net.NetworkInformation;
using System.Net.Sockets;
using RosSharp.wing_msg;
using System.ComponentModel;

/**
 * When "Just My Code" is enabled, 
 * Visual Studio in some cases will break on the line that throws the exception and display an error message that says "exception not handled by user code."
 * This error is benign. 
 * You can press F5 to continue and see the exception-handling behavior that is demonstrated in these examples.
 * To prevent Visual Studio from breaking on the first error, just uncheck the Enable Just My Code checkbox under Tools, Options, Debugging, General.
 */

namespace RosNode
{
    public class RosNode
    {
        protected Node n;

        [Description("Ros节点是否启动")]
        public bool NodeReady { get; private set; } = false;
        [Description("订阅者是否启动")]
        public bool SubReady { get; private set; } = false;
        [Description("发布者是否启动")]
        public bool PubReady { get; private set; } = false;

        protected RosNode()
        {
            SetRosMasterUri("127.0.0.1");
            Ros.TopicTimeout = 5000;
            Ros.XmlRpcTimeout = 5000;
            NodeReady = false;
        }

        public void SetRosMasterUri(string master_ip)
        {
            Ros.MasterUri = new Uri("http://" + master_ip + ":11311");
        }

        public bool Init()
        {
            bool ret = true;
            if (NodeReady)
            {
                Console.WriteLine("Ros Node Already Started.");
                return true;
            }
            Ros.HostName = GetLocalIP();  //本机IP地址
            Console.WriteLine("Start dotnet Ros node.");
            try
            {
                var t = Ros.InitNodeAsync("/dotnet_node");
                t.Wait();
                n = t.Result;
                Console.WriteLine("Connect to ROS Master Successfully.");
                NodeReady = true;
            }
            catch (AggregateException ae)
            {
                foreach (var e in ae.InnerExceptions)
                {
                    Console.WriteLine(e.Message);
                }
                return false;
            }
            return ret;
        }

        public virtual void RegisterSubs() { }

        public virtual void DisposeSubs() { }

        public virtual void RegisterPubs() { }

        public virtual void DisposePubs() { }

        public bool InitSubs()
        {
            if (!NodeReady)
            {
                Console.WriteLine("Ros Node Not Start Yet.");
                return false;
            }

            if (SubReady)
            {
                Console.WriteLine("Subscribers Already Started.");
                return false;
            }

            try
            {
                RegisterSubs();
                Console.WriteLine("Subscribers Start Successfully.");
                SubReady = true;
            }
            catch (AggregateException ae)
            {
                foreach (var e in ae.InnerExceptions)
                {
                    Console.WriteLine(e.Message);
                }
                SubReady = false;
                return false;
            }

            return true;
        }

        public bool InitPubs()
        {
            if (!NodeReady)
            {
                Console.WriteLine("Ros Node Not Start Yet.");
                return false;
            }
            if (PubReady)
            {
                Console.WriteLine("Publishers Already Started.");
                return false;
            }

            try
            {
                RegisterPubs();
                Console.WriteLine("Publishers Start Successfully.");
                PubReady = true;
            }
            catch (AggregateException ae)
            {
                foreach (var e in ae.InnerExceptions)
                {
                    Console.WriteLine(e.Message);
                }
                return false;
            }

            return true;
        }

        public void DeInit()
        {
            if (SubReady)
            {
                DisposeSubs();
                Console.WriteLine("Subscribers Disposed.");
            }
            if (PubReady)
            {
                DisposePubs();
                Console.WriteLine("Publishers Disposed");
            }
            if (NodeReady)
            {
                n.Dispose();
                Console.WriteLine("ROS node Disposed");
            }
            NodeReady = false;
            PubReady = false;
            SubReady = false;
        }

        public string GetLocalIP()
        {
            string ipv4 = "0.0.0.0";
            NetworkInterface[] nics = NetworkInterface.GetAllNetworkInterfaces();
            foreach (NetworkInterface adapter in nics)
            {
                //Wireless80211 无线网卡     Ethernet 以太网卡   
                if (adapter.NetworkInterfaceType == NetworkInterfaceType.Wireless80211)
                {

                    //网卡Mac地址
                    PhysicalAddress mac = adapter.GetPhysicalAddress();
                    //TODO:根据mac 地址 匹配目标网卡
                    if (mac.ToString() == "94B86D41D34F")
                    {
                        //获取以太网卡网络接口信息
                        IPInterfaceProperties ip = adapter.GetIPProperties();
                        //获取单播地址集
                        UnicastIPAddressInformationCollection ipCollection = ip.UnicastAddresses;
                        foreach (UnicastIPAddressInformation ipadd in ipCollection)
                        {
                            if (ipadd.Address.AddressFamily == AddressFamily.InterNetwork)
                            {
                                ipv4 = ipadd.Address.ToString();//获取ip
                                Console.WriteLine($"ROS Local IP: {ipv4}");
                            }
                        }
                    }

                }
            }
            return ipv4;
        }
    }
}
